#include <navigation/utils/local_planner/path_follower.h>

path_follower::path_follower(float max_x_vel, float min_x_vel, float max_x_acc, float max_yaw_vel, float min_yaw_vel, float max_yaw_acc, bool allow_reverse_drive,
                             bool use_advanced_evaluation, bool use_kinematic_limits, bool use_find_mode_follower,
                             float velocity_dec_offset_time,
                             float find_mode_x_vel, float find_mode_yaw_vel, float find_mode_radius, float find_mode_min_dist_timeout, bool enable_near_angle_correct,
                             float angle_diff_to_forward, float angle_diff_to_rotate,
                             float xy_tolerance, float yaw_tolerance,
                             float chassis_radius,
                             float sim_time, int sim_vx_space_size, int sim_vyaw_space_size, int trajectory_sim_step, float follow_point_weight, float follow_path_weight,
                             float stop_distance, float turning_stop_distance, float ignore_obstacle_dist,
                             float obstacle_acc_ratio,
                             float control_freq)
{
    set_kinematic_params(max_x_vel, max_yaw_vel);
    min_x_vel_ = min_x_vel;
    max_x_acc_ = max_x_acc;
    min_yaw_vel_ = min_yaw_vel;
    max_yaw_acc_ = max_yaw_acc;

    velocity_dec_offset_time_ = velocity_dec_offset_time;

    allow_reverse_drive_ = allow_reverse_drive;
    use_advanced_evaluation_ = use_advanced_evaluation;
    use_kinematic_limits_ = use_kinematic_limits;
    use_find_mode_follower_ = use_find_mode_follower;

    find_mode_x_vel_ = find_mode_x_vel;
    find_mode_yaw_vel_ = find_mode_yaw_vel;
    find_mode_radius_ = find_mode_radius;
    find_mode_min_dist_timeout_ = find_mode_min_dist_timeout;

    enable_near_angle_correct_ = enable_near_angle_correct;

    angle_diff_to_forward_ = angle_diff_to_forward;
    angle_diff_to_rotate_ = angle_diff_to_rotate;

    xy_tolerance_ = xy_tolerance;
    yaw_tolerance_ = yaw_tolerance;

    chassis_radius_ = chassis_radius;

    sim_time_ = sim_time;
    sim_vx_space_size_ = sim_vx_space_size;
    sim_vyaw_space_size_ = sim_vyaw_space_size;
    trajectory_sim_step_ = trajectory_sim_step;
    follow_point_weight_ = follow_point_weight;
    follow_path_weight_ = follow_path_weight;

    stop_distance_ = stop_distance;
    turning_stop_distance_ = turning_stop_distance;
    ignore_obstacle_dist_ = ignore_obstacle_dist;

    obstacle_acc_ratio_ = obstacle_acc_ratio;

    control_freq_ = control_freq;

    state_ = navigation_state::ready;
    obstalce_state_ = block_state::clear;
    last_state_ = state_;

    vx_delta_ = (1.0f / control_freq_) * max_x_acc_;
    vyaw_delta_ = (1.0f / control_freq_) * max_yaw_acc_;

    current_point_in_path_pub_ = node_handle_.advertise<geometry_msgs::PoseStamped>("current_point_in_path", 1);
    point_to_track_pub_ = node_handle_.advertise<geometry_msgs::PoseStamped>("point_to_track", 1);
    best_trajectory_pub_ = node_handle_.advertise<nav_msgs::Path>("best_trajectory", 1);
    all_trajectory_pub_ = node_handle_.advertise<nav_msgs::Path>("all_trajectory", 1);

    mover_ = unique_ptr<mover>(new mover(max_x_vel_, min_x_vel_, max_x_acc_, max_yaw_vel_, min_yaw_vel_, max_yaw_acc_,
                                         velocity_dec_offset_time_,
                                         velocity_dec_offset_time_,
                                         find_mode_min_dist_timeout_,
                                         xy_tolerance_, yaw_tolerance_, stop_distance_, 0, 0,
                                         obstacle_acc_ratio_,
                                         control_freq_));

    acc_controlled_ = false;
    last_acc_controlled_ = false;

    find_mode_ = false;
    near_angle_corrected_ = false;
    stand_at_goal_ = false;

    find_mode_counter_ = 0;
    dist_ = 0;
    min_dist_ = INFINITY;

    rotation_state_ = rotation_state::ready;
}

path_follower::~path_follower()
{
    if (mover_ != NULL)
    {
        mover_.reset();
    }
}

void path_follower::set_kinematic_params(float max_x_vel, float max_yaw_vel)
{
    max_x_vel_ = max_x_vel;
    max_yaw_vel_ = max_yaw_vel;
}

bool path_follower::make_plan(geometry_msgs::PoseWithCovarianceStamped pose, vector<geometry_msgs::PoseStamped> plan, bool ignore_obstacle)
{
    bool ret = false;

    if (state_ == navigation_state::ready)
    {
        plan_ = plan;
        pose_ = pose;

        if (plan_.size() > 0)
        {
#if PF_DEBUG
            ROS_INFO("新导航任务已接收，路径点数量：%d。", (int)plan_.size());
#endif
            // 设置标记
            ignore_obstacle_ = ignore_obstacle;
            current_point_in_path_ = 0;
            find_mode_ = false;
            near_angle_corrected_ = false;
            stand_at_goal_ = false;
            acc_controlled_ = false;
            last_acc_controlled_ = false;
            find_mode_counter_ = 0;
            dist_ = 0;
            min_dist_ = INFINITY;
            obstalce_state_ = block_state::clear;
            rotation_state_ = rotation_state::ready;

            mover_->reset_state();

            // 记录当前目标点
            goal_ = plan_[plan_.size() - 1].pose;

            // 重置最优局部路径
            current_best_trajectory_.header.frame_id = plan_[plan_.size() - 1].header.frame_id;
            current_best_trajectory_.poses.clear();

            // 判断是否需要向后驱动
            if (allow_reverse_drive_)
            {
                reverse_drive_ = false;

                // 计算正向驱动时预先需要旋转的角度
                float angle_to_goal = calc_goal_angle();

                // 如果正向驱动需要旋转的角度大于90度，则选择向后驱动
                if (abs(angle_to_goal) > M_PI / 2)
                {
#if PF_DEBUG
                    ROS_INFO("旋转角度为：%f，选择向后驱动。", angle_to_goal);
#endif

                    reverse_drive_ = true;
                }
            }
            else
            {
                reverse_drive_ = false;
            }

            state_ = navigation_state::processing;

            ret = true;
        }
        else
        {
#if PF_DEBUG
            ROS_WARN("路径为空，无法进行导航。");
#endif
        }
    }

    return ret;
}

bool path_follower::start_session()
{
    bool ret = false;

    if (state_ == navigation_state::processing)
    {
        state_ = navigation_state::turning;

        ret = true;
    }

    return ret;
}

bool path_follower::pause_session()
{
    bool ret = false;

    if (state_ != navigation_state::ready &&
        state_ != navigation_state::processing &&
        state_ != navigation_state::reached &&
        state_ != navigation_state::pausing &&
        state_ != navigation_state::blocked)
    {
        last_twist_target_ = twist_target_;

        last_state_ = state_;
        state_ = navigation_state::pausing;

        ret = true;
    }

    return ret;
}

bool path_follower::continue_session()
{
    bool ret = false;

    if (state_ == navigation_state::pausing)
    {
        state_ = last_state_;

        twist_target_ = last_twist_target_;

        ret = true;
    }

    return ret;
}

navigation_state path_follower::loop(geometry_msgs::PoseWithCovarianceStamped pose,
                                     geometry_msgs::TwistWithCovarianceStamped odom_twist,
                                     tecbot_msgs::obstacle obstacle)
{
    // 处理事件
    if (state_ != navigation_state::ready &&
        state_ != navigation_state::reached &&
        state_ != navigation_state::processing &&
        state_ != navigation_state::pausing)
    {
        // 更新数据
        pose_ = pose;
        odom_twist_ = odom_twist;
        obstacle_ = obstacle;

        // 更新机器人在全局路径中的点
        update_current_point_in_path();

        // 更新到目标点的距离
        dist_ = distance(pose_.pose.pose, goal_);

        // 第一步，旋转至接近路径方向
        if (state_ == navigation_state::turning &&
            obstalce_state_ == block_state::clear)
        {
            if (rotation_state_ == rotation_state::ready)
            {
                // 计算所需要的旋转角度
                float angle_to_global_path = calc_global_plan_angle(get_max_sim_distance_point_index());

                // 旋转至开始行驶容差内
                rotate(angle_to_global_path, yaw_tolerance_, max_yaw_vel_);
            }
            else if (rotate())
            {
#if PF_DEBUG
                ROS_INFO("全局路径跟随旋转完毕，准备开始行驶。");
#endif

                state_ = navigation_state::strighting;
            }
        }
        // 第二步，跟随路径行驶
        else if (state_ == navigation_state::strighting &&
                 obstalce_state_ == block_state::clear)
        {
            // 若未到达目标点或仍在旋转，跟随路径行驶
            if (!stand_at_goal_ ||
                rotation_state_ != rotation_state::ready)
            {
                // 定义当前状态下的最大速度
                float max_x_vel = find_mode_x_vel_;
                float max_yaw_vel = find_mode_yaw_vel_;

                // 定义当前状态下旋转误差
                float yaw_tolrance = yaw_tolerance_;

                // 定义当前状态下旋转阈值
                float angle_to_rotate =
                    near_angle_corrected_ ? angle_diff_to_rotate_ : yaw_tolerance_;

                // 定义最远的可模拟计算的插值点为目标点
                int max_sim_point_index = plan_.size() - 1;

                // 定义距离目标点的距离
                volatile float dist = distance(goal_, pose_.pose.pose);

                // 计算距离目标点的角度
                volatile float angle = calc_goal_angle();

                // 设置进入目标寻找模式标志
                find_mode_ = true;

                // 若在目标寻找半径外，则未进入目标寻找模式
                if (dist_ > find_mode_radius_)
                {
                    // 搜索最远的可模拟计算的插值点
                    max_sim_point_index = get_max_sim_angle_point_index(get_max_sim_distance_point_index());

                    // 设置最大速度
                    max_x_vel = max_x_vel_;
                    max_yaw_vel = max_yaw_vel_;

                    // 设置旋转误差
                    yaw_tolrance = angle_diff_to_forward_;

                    // 设置旋转阈值
                    angle_to_rotate = angle_diff_to_rotate_;

                    // 计算距离下一个插值点的距离
                    dist = distance(plan_[max_sim_point_index].pose, pose_.pose.pose);

                    // 计算距离下一个插值点的角度
                    angle = calc_global_plan_angle(max_sim_point_index);

                    // 当前非寻找模式
                    find_mode_counter_ = 0;
                    find_mode_ = false;
                }

                // 如果偏离路径过大，重新修正角度
                if (((!find_mode_ || (find_mode_ && enable_near_angle_correct_ && !near_angle_corrected_)) && abs(angle) > angle_to_rotate) ||
                    rotation_state_ != rotation_state::ready)
                {
                    if (rotation_state_ == rotation_state::ready)
                    {
                        // 旋转至开始行驶容差内
                        rotate(angle, yaw_tolrance, max_yaw_vel);
                    }
                    else if (rotate())
                    {
#if PF_DEBUG
                        ROS_INFO("全局路径跟随旋转完毕，准备开始行驶。");
#endif

                        if (find_mode_ && !near_angle_corrected_)
                        {
                            near_angle_corrected_ = true;
                        }

                        find_mode_counter_ = 0;
                    }
                }
                else
                {
                    // 计算新速度
                    float vx = 0, vyaw = 0;

                    // 进行航迹推算计算速度
                    trajectory_simulation(vx, vyaw, max_sim_point_index, max_x_vel, max_yaw_vel);

                    // 若超过运动学特性，进行动态速度分配
                    if (use_kinematic_limits_)
                    {
                        volatile float combined_vel = abs(vx) + abs(vyaw * chassis_radius_);

                        if (combined_vel > max_x_vel)
                        {
                            volatile float vel_factor = max_x_vel / combined_vel;
                            vx *= vel_factor;
                            vyaw *= vel_factor;
                        }
                    }

                    // 防止丢失线速度，永远无法到达
                    if (abs(vx) < min_x_vel_)
                    {
                        vx = copysign(min_x_vel_, vx);
                    }

                    // 设置计算速度为目标速度
                    twist_target_.linear.x = vx;
                    twist_target_.angular.z = vyaw;

#if PF_DEBUG
                    // ROS_INFO("误差：%f，线速度：%f，角速度：%f。",
                    //          dist_, vx, vyaw);
#endif

                    // 寻找超时计数器
                    if (find_mode_ &&
                        ((abs(odom_twist_.twist.twist.linear.x) >= min_x_vel_ / 2.0f ||
                          abs(odom_twist_.twist.twist.angular.z) >= min_yaw_vel_ / 2.0f)) &&
                        dist_ > min_dist_)
                    {
                        find_mode_counter_++;
                    }
                    else
                    {
                        find_mode_counter_ = 0;
                    }

                    // 如果到达直线容差，则认为已经在目标点停止
                    if (find_mode_counter_ >= (int)abs(find_mode_min_dist_timeout_ * control_freq_) ||
                        dist_ <= xy_tolerance_)
                    {
                        stand_at_goal_ = true;
                    }
                }
            }
            // 若到达目标点，转向至正确方向
            else
            {
#if PF_DEBUG
                ROS_INFO("全局路径跟随行驶完毕，准备调整最终角度。");
#endif

                acc_controlled_ = true;
                twist_target_.linear.x = 0;
                twist_target_.angular.z = 0;

                state_ = navigation_state::turning_recovery;
            }
        }
        // 第三步，调整角度
        else if (state_ == navigation_state::turning_recovery &&
                 obstalce_state_ == block_state::clear)
        {
            // 最终角度调整
            if (rotation_state_ == rotation_state::ready)
            {
                // 注意此处，无论是否向后驱动，最终朝向角均以正方向为准。
                float angle_to_goal = (float)angles::shortest_angular_distance(
                    tf::getYaw(pose_.pose.pose.orientation), tf::getYaw(goal_.orientation));

                // 旋转至最终角度容差内
                rotate(angle_to_goal, yaw_tolerance_, find_mode_yaw_vel_);
            }
            else if (rotate())
            {
#if PF_DEBUG
                ROS_INFO("最终角度调整完毕，导航结束。");
#endif

                acc_controlled_ = true;
                twist_target_.linear.x = 0;
                twist_target_.angular.z = 0;

                obstalce_state_ = block_state::clear;
                state_ = navigation_state::reached;
            }
        }
        else if (state_ == navigation_state::blocked &&
                 obstalce_state_ == block_state::clear)
        {
            twist_target_ = last_twist_target_;
            vx_delta_ /= obstacle_acc_ratio_;
            vyaw_delta_ /= obstacle_acc_ratio_;
            acc_controlled_ = last_acc_controlled_;
            state_ = last_state_before_blocked_;
        }
        else if (state_ != navigation_state::blocked &&
                 obstalce_state_ != block_state::clear)
        {
            last_twist_target_ = twist_target_;

            vx_delta_ *= obstacle_acc_ratio_;
            vyaw_delta_ *= obstacle_acc_ratio_;

            twist_target_.linear.x = 0;
            twist_target_.angular.z = 0;

            last_acc_controlled_ = acc_controlled_;
            acc_controlled_ = false;

            last_state_before_blocked_ = state_;

            state_ = navigation_state::blocked;
        }

        // 更新最小距离
        min_dist_ = min(dist_, min_dist_);

        // 检测障碍物
        obstalce_state_ = is_obstacle_detected();
    }
    else if (state_ == navigation_state::pausing)
    {
        twist_target_.linear.x = 0;
        twist_target_.angular.z = 0;
    }

    return state_;
}

void path_follower::update_current_point_in_path()
{
    float min_dist_to_base = INFINITY;

    // 向前搜索
    for (int i = current_point_in_path_; i < plan_.size(); i++)
    {
        float dist = distance(plan_[i].pose, pose_.pose.pose);

        // 判断路径中的点距离是否更接近机器人本体
        if (dist < min_dist_to_base)
        {
            current_point_in_path_ = i;
            min_dist_to_base = dist;
        }

        // 搜索终点
        if (dist >= sim_time_ * max_x_vel_)
        {
            break;
        }
    }

    // 向后搜索
    for (int i = current_point_in_path_; i >= 0; i--)
    {
        float dist = distance(plan_[i].pose, pose_.pose.pose);

        // 判断路径中的点距离是否更接近机器人本体
        if (dist < min_dist_to_base)
        {
            current_point_in_path_ = i;
            min_dist_to_base = dist;
        }

        // 搜索终点
        if (dist >= sim_time_ * max_x_vel_)
        {
            break;
        }
    }

    // 发布点
    current_point_in_path_pub_.publish(plan_[current_point_in_path_]);
}

bool path_follower::rotate(float angle, float tolerance, float max_yaw_vel)
{
    bool ret = false;

    if (rotation_state_ == rotation_state::ready)
    {
        // 调整角度前先停车
        twist_target_.linear.x = 0;
        twist_target_.angular.z = 0;

        if (abs(odom_twist_.twist.twist.linear.x) <= vx_delta_ / 2.0f &&
            abs(odom_twist_.twist.twist.angular.z) <= vyaw_delta_ / 2.0f)
        {
            // 设置旋转参数
            mover_->reset_state();
            mover_->set_kinematic_params(max_x_vel_, max_yaw_vel);
            mover_->set_tolerance_params(xy_tolerance_, tolerance);

            // 规划旋转速度
            mover_->make_plan(pose_, 0, angle, true);

            // 开始旋转
            mover_->start_session();

            // 设置加速控制为外部接管模式
            acc_controlled_ = true;

            rotation_state_ = rotation_state::running;
        }
    }
    else if (rotation_state_ == rotation_state::running)
    {
        // 处理旋转控制事件
        auto state = mover_->loop(pose_, odom_twist_, obstacle_);

        // 如果旋转状态为到达
        if (state == navigation_state::reached)
        {
            // 重置旋转控制器
            mover_->reset_state();

            // 机器人停车
            twist_target_.linear.x = 0;
            twist_target_.angular.z = 0;

            rotation_state_ = rotation_state::reached;
        }
        else
        {
            twist_target_ = mover_->get_current_twist();
        }
    }
    else if (rotation_state_ == rotation_state::reached)
    {
        // 等待机器人彻底停车
        ros::Duration((min_yaw_vel_ * 2.0f) / max_yaw_acc_).sleep();

        // 设置加速度控制为内部模式
        acc_controlled_ = false;

        // 恢复状态
        rotation_state_ = rotation_state::ready;

#if PF_DEBUG
        ROS_INFO("旋转状态已经重置。");
#endif

        ret = true;
    }

    return ret;
}

geometry_msgs::Twist path_follower::get_current_twist()
{
    // 如果加速度为外部接管模式
    if (acc_controlled_)
    {
        // 速度为目标速度
        twist_.linear.x = twist_target_.linear.x;
        twist_.angular.z = twist_target_.angular.z;
    }
    // 否则进行加减速控制
    else
    {
        // 控制线速度
        if (odom_twist_.twist.twist.linear.x < twist_target_.linear.x)
        {
            twist_.linear.x += vx_delta_;

            if (twist_.linear.x > twist_target_.linear.x)
            {
                twist_.linear.x = twist_target_.linear.x;
            }
        }
        else if (odom_twist_.twist.twist.linear.x > twist_target_.linear.x)
        {
            twist_.linear.x -= vx_delta_;

            if (twist_.linear.x < twist_target_.linear.x)
            {
                twist_.linear.x = twist_target_.linear.x;
            }
        }

        // 控制角速度
        if (odom_twist_.twist.twist.angular.z < twist_target_.angular.z)
        {
            twist_.angular.z += vyaw_delta_;

            if (twist_.angular.z > twist_target_.angular.z)
            {
                twist_.angular.z = twist_target_.angular.z;
            }
        }
        else if (odom_twist_.twist.twist.angular.z > twist_target_.angular.z)
        {
            twist_.angular.z -= vyaw_delta_;

            if (twist_.angular.z < twist_target_.angular.z)
            {
                twist_.angular.z = twist_target_.angular.z;
            }
        }
    }

    return twist_;
}

geometry_msgs::PoseWithCovarianceStamped path_follower::get_current_pose()
{
    return pose_;
}

geometry_msgs::PoseStamped path_follower::get_current_path_point_pose()
{
    return plan_[current_point_in_path_];
}

block_state path_follower::get_obstacle_state()
{
    return obstalce_state_;
}

void path_follower::reset_state()
{
    twist_target_.linear.x = 0;
    twist_target_.angular.z = 0;
    current_point_in_path_ = 0;
    obstalce_state_ = block_state::clear;
    state_ = navigation_state::ready;
}

int path_follower::get_max_sim_distance_point_index()
{
    int ret = -1;

    // 在路径中寻找模拟时间内可预计算的距离最远点
    for (int i = current_point_in_path_; i < plan_.size(); i++)
    {
        // 计算当前位姿到全局路径中点的距离
        float dist = distance(pose_.pose.pose, plan_[i].pose);

        if (dist > max_x_vel_ * sim_time_)
        {
            ret = (i <= 0 ? 0 : (i - 1));

            break;
        }
    }

    // 如果未在路径中找到，则为路径中最后一个点
    if (ret == -1)
    {
        ret = plan_.size() - 1;
        ret = ret < 0 ? 0 : ret;
    }

    return ret;
}

int path_follower::get_max_sim_angle_point_index(int point_index)
{
    int ret = point_index;

    // 在路径中寻找模拟时间内可预计算的角度最远点
    for (int i = point_index; i >= current_point_in_path_; i--)
    {
        // 计算当前位姿到全局路径中点的距离
        float angle = calc_global_plan_angle(i);

        if (abs(angle) <= max_yaw_vel_ * sim_time_)
        {
            ret = i;
            break;
        }
    }

    // 发布点
    point_to_track_pub_.publish(plan_[ret]);

    return ret;
}

float path_follower::calc_goal_angle()
{
    float ret = 0;

    // 获取当前航向角
    float current_yaw = angles::normalize_angle(
        tf::getYaw(pose_.pose.pose.orientation) + (reverse_drive_ ? M_PI : 0));

    // 获取到该点的直行角度
    float angle_to_goal = atan2(goal_.position.y - pose_.pose.pose.position.y,
                                goal_.position.x - pose_.pose.pose.position.x);

    // 计算到全局路径的角度差
    ret = (float)angles::shortest_angular_distance(current_yaw, angle_to_goal);

    return ret;
}

float path_follower::calc_global_plan_angle(int point_index)
{
    float ret = 0;

    float angle_to_path = 0;

    int point_num = 0;

    if (plan_.size() <= 1)
    {
        return calc_goal_angle();
    }

    // 获取当前航向角
    float current_yaw = angles::normalize_angle(
        tf::getYaw(pose_.pose.pose.orientation) + (reverse_drive_ ? M_PI : 0));

    for (int i = current_point_in_path_; i <= point_index; i++)
    {
        // 获取到该点的直行角度
        float angle_to_point =
            (float)angles::shortest_angular_distance(current_yaw, atan2(plan_[i].pose.position.y - pose_.pose.pose.position.y,
                                                                        plan_[i].pose.position.x - pose_.pose.pose.position.x));

        // 累加角度
        angle_to_path += angle_to_point;

        point_num++;
    }

    // 计算这一段内角度平均值
    angle_to_path /= (point_num == 0 ? 1.0f : (float)point_num);

    // 计算到全局路径的角度差
    ret = angle_to_path;

    return ret;
}

void path_follower::trajectory_simulation(float &vx, float &vyaw,
                                          int max_sim_point_index,
                                          float max_x_vel, float max_yaw_vel)
{
    // 定义最优轨迹与当前轨迹
    nav_msgs::Path best_trajectory;
    nav_msgs::Path current_trajectory;

    // 计算距离下一个插值点的距离
    float dist = distance(plan_[max_sim_point_index].pose, pose_.pose.pose);

    // 获取当前航向角
    float current_yaw = tf::getYaw(pose_.pose.pose.orientation);

    // 获取到目标点的角度
    float angle_to_goal = calc_goal_angle();

    // 设置搜索线速度范围
    float current_vx = odom_twist_.twist.twist.linear.x;
    float start_search_vx = max(min_x_vel_, current_vx - sim_time_ * max_x_acc_);
    float end_search_vx = max(start_search_vx, min(current_vx + sim_time_ * max_x_acc_, dist / sim_time_));

    start_search_vx *= (reverse_drive_ ? -1.0f : 1.0f);
    end_search_vx *= (reverse_drive_ ? -1.0f : 1.0f);

    float delta_search_vx = (end_search_vx - start_search_vx) / (sim_vx_space_size_ <= 0 ? 1 : sim_vx_space_size_);
    float target_search_vx = start_search_vx;

    // 设置角速度搜索范围
    float current_vyaw = odom_twist_.twist.twist.angular.z;
    float start_search_vyaw = current_vyaw - sim_time_ * max_yaw_acc_;
    float end_search_vyaw = current_vyaw + sim_time_ * max_yaw_acc_;

    float delta_search_vyaw = (end_search_vyaw - start_search_vyaw) / (sim_vyaw_space_size_ <= 0 ? 1 : sim_vyaw_space_size_);
    float target_search_vyaw = start_search_vyaw;

    // 计算微分时间
    float delta_sim_time = sim_time_ / (trajectory_sim_step_ <= 0 ? 1 : trajectory_sim_step_);

    // 计算推算加速度微分
    float delta_current_search_vx = max_x_acc_ * delta_sim_time;
    float delta_current_search_vyaw = max_yaw_acc_ * delta_sim_time;

    // 设置最大评分
    float max_score = -INFINITY;

    // 搜索线速度空间
    for (int i = -1; i < sim_vx_space_size_; i++)
    {
        if (i == -1)
        {
            target_search_vx = 0;
        }
        else if (i == 0)
        {
            target_search_vx = start_search_vx;
        }

        // 搜索角速度空间
        for (int j = -1; j < sim_vyaw_space_size_; j++)
        {
            if (j == -1)
            {
                target_search_vyaw = 0;
            }
            else if (j == 0)
            {
                target_search_vyaw = start_search_vyaw;
            }

            // 设置推算初始速度
            float current_search_vx = use_kinematic_limits_ ? odom_twist_.twist.twist.linear.x : target_search_vx;
            float current_search_vyaw = use_kinematic_limits_ ? odom_twist_.twist.twist.angular.z : target_search_vyaw;
            float last_search_vx = current_search_vx;
            float last_search_vyaw = current_search_vyaw;

            // 定义在推算中经过的角度
            float rotated_sim_yaw = 0;

            // 将当前点加入推算轨迹
            current_trajectory.poses.clear();
            geometry_msgs::PoseStamped current_pose;
            current_pose.pose = pose_.pose.pose;
            current_trajectory.poses.push_back(current_pose);

            // 进行航迹推算
            for (int k = 0; k < trajectory_sim_step_; k++)
            {
                if (use_kinematic_limits_)
                {
                    // 更新推算线速度
                    if (current_search_vx < target_search_vx)
                    {
                        current_search_vx += delta_current_search_vx;

                        if (current_search_vx > target_search_vx)
                        {
                            current_search_vx = target_search_vx;
                        }
                    }
                    else if (current_search_vx > target_search_vx)
                    {
                        current_search_vx -= delta_current_search_vx;

                        if (current_search_vx < target_search_vx)
                        {
                            current_search_vx = target_search_vx;
                        }
                    }

                    // 更新推算角速度
                    if (current_search_vyaw < target_search_vyaw)
                    {
                        current_search_vyaw += delta_current_search_vyaw;

                        if (current_search_vyaw > target_search_vyaw)
                        {
                            current_search_vyaw = target_search_vyaw;
                        }
                    }
                    else if (current_search_vyaw > target_search_vyaw)
                    {
                        current_search_vyaw -= delta_current_search_vyaw;

                        if (current_search_vyaw < target_search_vyaw)
                        {
                            current_search_vyaw = target_search_vyaw;
                        }
                    }
                }

                // 计算推算轨迹点
                geometry_msgs::PoseStamped current_sim_pose;
                current_sim_pose.pose.orientation = tf::createQuaternionMsgFromYaw(0);

                current_sim_pose.pose.position.x =
                    current_trajectory.poses[k].pose.position.x +
                    (((last_search_vx + current_search_vx) / 2.0f) * delta_sim_time) * cos(current_yaw + rotated_sim_yaw);
                current_sim_pose.pose.position.y =
                    current_trajectory.poses[k].pose.position.y +
                    (((last_search_vx + current_search_vx) / 2.0f) * delta_sim_time) * sin(current_yaw + rotated_sim_yaw);

                // 更新旋转的角度
                rotated_sim_yaw += ((last_search_vyaw + current_search_vyaw) / 2.0f) * delta_sim_time;

                // 记录上一次速度
                last_search_vx = current_search_vx;
                last_search_vyaw = current_search_vyaw;

                // 将推算点加入路径中
                current_trajectory.poses.push_back(current_sim_pose);
            }

            current_trajectory.header.frame_id = current_best_trajectory_.header.frame_id;
            current_trajectory.header.stamp = ros::Time::now();
            all_trajectory_pub_.publish(current_trajectory);

            // 计算路径终点到跟踪点距离
            float dist_to_point = distance(current_trajectory.poses[current_trajectory.poses.size() - 1].pose,
                                           plan_[max_sim_point_index].pose);

            float current_score = 0;

            // 如果为寻找模式或路径中只有一个点，则只考虑对点的跟随性
            if ((use_find_mode_follower_ && find_mode_) || max_sim_point_index == plan_.size() - 1)
            {
                current_score =
                    1.0f / (follow_point_weight_ * dist_to_point);
            }
            else
            {
                // 计算当前路径到全局路径的距离
                float mean_dist_to_path = use_advanced_evaluation_ ? 0 : INFINITY;

                if (!use_advanced_evaluation_)
                {
                    for (int k = current_point_in_path_; k < plan_.size(); k++)
                    {
                        float dist = distance(current_trajectory.poses[current_trajectory.poses.size() - 1].pose,
                                              plan_[k].pose);

                        mean_dist_to_path = min(mean_dist_to_path, dist);

                        if (dist > max_x_vel * sim_time_)
                        {
                            break;
                        }
                    }
                }
                else
                {
                    // 寻找当前推算轨迹末端点
                    float min_dist_to_path = INFINITY;

                    int end_point_in_path = current_point_in_path_;

                    for (int k = current_point_in_path_; k < plan_.size(); k++)
                    {
                        float dist = distance(current_trajectory.poses[current_trajectory.poses.size() - 1].pose,
                                              plan_[k].pose);

                        if (dist <= min_dist_to_path)
                        {
                            end_point_in_path = k;
                            min_dist_to_path = dist;
                        }

                        if (dist > max_x_vel * sim_time_)
                        {
                            break;
                        }
                    }

                    // 轨迹均匀分布
                    float index_increment_ratio =
                        1.0f * (end_point_in_path - current_point_in_path_ + 1) / (current_trajectory.poses.size() + 1);
                    int path_point_index = current_point_in_path_;
                    int trajectory_point_index = 0;
                    int dist_counter = 0;

                    mean_dist_to_path = 0;

                    while (path_point_index <= end_point_in_path && trajectory_point_index < current_trajectory.poses.size())
                    {
                        mean_dist_to_path += distance(plan_[path_point_index].pose,
                                                      current_trajectory.poses[trajectory_point_index].pose);
                        dist_counter++;

                        path_point_index += (int)(index_increment_ratio);
                        trajectory_point_index++;
                    }

                    mean_dist_to_path /= (dist_counter == 0 ? 1.0f : (1.0f * dist_counter));
                }

                // 计算评分
                current_score =
                    1.0f / ((follow_point_weight_ * dist_to_point + follow_path_weight_ * mean_dist_to_path) /
                            (follow_point_weight_ + follow_path_weight_ + 1e-5));
            }

            // 计算是否当前轨迹评分更优
            if (current_score >= max_score)
            {
                // 更新评分
                max_score = current_score;

                // 更新路径
                best_trajectory.poses = current_trajectory.poses;

                // 更新速度
                vx = target_search_vx;
                vyaw = target_search_vyaw;
            }

            // 更新角速度
            target_search_vyaw += delta_search_vyaw;
        }

        // 更新线速度
        target_search_vx += delta_search_vx;

        // 恢复角速度
        target_search_vyaw = start_search_vyaw;
    }

    // 发布最优局部路径
    current_best_trajectory_.header.stamp = ros::Time::now();
    current_best_trajectory_.poses = best_trajectory.poses;
    best_trajectory_pub_.publish(current_best_trajectory_);
}

block_state path_follower::is_obstacle_detected()
{
    block_state ret = block_state::clear;

    if (max(abs(twist_target_.linear.x), abs(last_twist_target_.linear.x)) >= vx_delta_ / 2)
    {
        if (max(last_twist_target_.linear.x, twist_target_.linear.x) > 0)
        {
            for (int i = 0; i < obstacle_.size; i++)
            {
                if (obstacle_.obstacle_front[i] > 0 &&
                    obstacle_.obstacle_front[i] < (ignore_obstacle_ ? ignore_obstacle_dist_ : stop_distance_) &&
                    obstacle_.obstacle_front[i] < distance(pose_.pose.pose, goal_))
                {
                    ret = block_state::forward;
                    break;
                }
            }
        }
        else
        {
            for (int i = 0; i < obstacle_.size; i++)
            {
                if (obstacle_.obstacle_rear[i] > 0 &&
                    obstacle_.obstacle_rear[i] < (ignore_obstacle_ ? ignore_obstacle_dist_ : stop_distance_) &&
                    obstacle_.obstacle_rear[i] < distance(pose_.pose.pose, goal_))
                {
                    ret = block_state::backward;
                    break;
                }
            }
        }
    }
    else if (max(abs(twist_target_.angular.z), abs(last_twist_target_.angular.z)) >= vyaw_delta_ / 2)
    {
        if (max(last_twist_target_.angular.z, twist_target_.angular.z) > 0)
        {
            for (int i = 0; i < obstacle_.size; i++)
            {
                if (obstacle_.obstacle_left[i] > 0 &&
                    obstacle_.obstacle_left[i] < (ignore_obstacle_ ? ignore_obstacle_dist_ : turning_stop_distance_))
                {
                    ret = block_state::left;
                    break;
                }
            }
        }
        else
        {
            for (int i = 0; i < obstacle_.size; i++)
            {
                if (obstacle_.obstacle_right[i] > 0 &&
                    obstacle_.obstacle_right[i] < (ignore_obstacle_ ? ignore_obstacle_dist_ : turning_stop_distance_))
                {
                    ret = block_state::right;
                    break;
                }
            }
        }
    }

    return ret;
}

float path_follower::distance(geometry_msgs::Pose pose0, geometry_msgs::Pose pose1)
{
    float ret = 0;

    ret = sqrt(pow(pose0.position.x - pose1.position.x, 2) +
               pow(pose0.position.y - pose1.position.y, 2) +
               pow(pose0.position.z - pose1.position.z, 2));

    return ret;
}